import math
import os
from ament_index_python import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node, PushROSNamespace
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import PathJoinSubstitution
from launch_ros.substitutions import FindPackageShare
from launch.actions import IncludeLaunchDescription, GroupAction

import numpy as np


def generate_launch_description():
    """
    创建一个带有两个使用外部控制逻辑的机器人仿真环境
    为每个机器人配置一个外部控制器和轨迹跟踪器
    创建局部路径规划器，生成局部轨迹
    """
    self_package_path = get_package_share_directory("synthesize")
    rviz__file = os.path.join(self_package_path, "config", "dock_view.rviz")

    inputJS = Node(
        package="input",
        executable="joystick",
        name="js_input",
        output="screen",
        parameters=[
            {
                "forward_scale": float(np.deg2rad(20)),
                "rotate_scale": float(np.deg2rad(30)),
            }
        ],
    )

    rviz2_node = Node(
        package="rviz2",
        executable="rviz2",
        name="rviz2",
        output="screen",
        arguments=["-d", rviz__file],
    )

    command = Node(
        package="synthesize", executable="cc", name="command_central", output="screen"
    )

    instance_manager = Node(
        package="cooperate",
        executable="instance_manager",
        name="instance_manager",
        output="screen",
    )

    auto_dock = Node(
        package="cooperate",
        executable="auto_dock",
        name="auto_dock",
        output="screen",
        parameters=[
            {
                "robot1": "robot1",
                "robot2": "robot2",
                "target1": "robot1/odom",
                "target2": "robot1/tag_odom",
            }
        ],
    )

    plot1 = Node(
        package="toolbox",
        executable="plot_runtime",
        name="plot_runtime",
        output="screen",
        parameters=[
            {
                "topics": "torque,rotate_torque,target_angle,angle,target_gyro,gyro,target_gyroz,gyroz,target_speed,speed"
            },
            {"auto_start": True},
        ],
        namespace="robot1",
    )

    plot2 = Node(
        package="toolbox",
        executable="plot_runtime",
        name="plot_runtime",
        output="screen",
        parameters=[
            {
                "topics": "torque,rotate_torque,target_angle,angle,target_gyro,gyro,target_gyroz,gyroz,target_speed,speed"
            },
            {"auto_start": True},
        ],
        namespace="robot2",
    )

    return LaunchDescription(
        [
            inputJS,
            rviz2_node,
            command,
            instance_manager,
            auto_dock,
            # plot1,
            # plot2,
        ]
    )
